/* ######################################################################### */
/* \file auxDynObjStr.hpp
 * \brief	This file contains auxiliary structures declarations
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      Auxiliary structures declarations
 *
// ######################################################################### */



//Matrix library
#include "newmat/newmatap.h"
#include "newmat/newmatio.h"

using namespace NEWMAT;

namespace DATMO{


struct DynObjConst
{
    //Smallest difference between two laser measurements in the same point of the matrix
    //from which you mark the point as changed (in meters)
    float f_laser_laser_dif_tol;// = 0.0125f;
    //Percentage of the scan from which you decide that too much has changed and the object recognition is not valid
    //eg. If more thatn this percentage of pixels has changed something is wrong and you don't perform the algorithm
    float f_laser_scan_percentage_tol;// = 0.3f;
    //Minimum number of pixels that an object needs to be consider a dynamic object
    int i_min_number_pixel;// = 5;
    //Difference between the laser measurement and the reference from which you consider that the pixel
    //is not part of the object but of the background (in meters)
    float f_laser_reference_dif_tol;// = 0.9f;
    //Tolerance in object sizes (in meters)
    float f_x_size_tol;// = 1;
    float f_y_size_tol;// = 1;
    float f_z_size_tol;// = 1;
    //Distance tolerance between two objects in order to consider that the selected pair is viable and is the
    //same object (in meters)
    float f_object_distance_tol;// = 100;
    //Maximum time that an object remains in the dynamic object list without being detected (in seconds)
    float t_min_detect;// = 10;
    //maximum number of objects in the list at the same time
    int i_max_object_number;

    DynObjConst()
    {
    	//Smallest difference between two laser measurements in the same point of the matrix
    	    //from which you mark the point as changed (in meters)
    	    f_laser_laser_dif_tol = 0.05;//0.0125f;
    	    //Percentage of the scan from which you decide that too much has changed and the object recognition is not valid
    	    //eg. If more thatn this percentage of pixels has changed something is wrong and you don't perform the algorithm
    	    f_laser_scan_percentage_tol = 0.3f;
    	    //Minimum number of pixels that an object needs to be consider a dynamic object
    	    i_min_number_pixel = 5;
    	    //Difference between the laser measurement and the reference from which you consider that the pixel
    	    //is not part of the object but of the background (in meters)
    	    f_laser_reference_dif_tol = 0.9f;
    	    //Tolerance in object sizes (in meters)
    	    f_x_size_tol = 1;
    	    f_y_size_tol = 1;
    	    f_z_size_tol = 1;
    	    //Distance tolerance between two objects in order to consider that the selected pair is viable and is the
    	    //same object (in meters)
    	    f_object_distance_tol = 100;
    	    //Maximum time that an object remains in the dynamic object list without being detected (in seconds)
    	    t_min_detect = 10;
    	    //maximum number of objects in the list at the same time
    	    i_max_object_number = 30;
    };
};



struct str_StaticDetectionData{
  int iteration;
  DistanceImage DistImage;
  DistanceImage LastDistImage;
  DistanceImage LastLastDistImage;
  BackgroundReference BackgroundRef;
  cl_DynamicObjects2 DynObjects;
};

struct str_DynamicDetectionData{

};

/* ------------------------------------------------------------------------- */
/**     \struct RobotState
 *
 *  \brief Auxiliary Structure containing information needed to perform
 *  dynamic object detection and identification
 */
/* ------------------------------------------------------------------------- */

struct RobotState{
  bool b_isMovement;
  int  s_Initial;
  string RobotName;
  int RobotID;
  tf::TransformListener tf_listener;
};



/* ------------------------------------------------------------------------- */
/**	\struct CollisionPoint
 *
 *  \brief Auxiliary Structure
 */
/* ------------------------------------------------------------------------- */
struct CollisionPoint{
	bool b_isPoint;
	float f_x;
	float f_y;
	float f_z;
};

/* ------------------------------------------------------------------------- */
/**	\struct DistanceImage
 *
 *  \brief Auxiliary Structure containing information of a DistanceImage
 */
/* ------------------------------------------------------------------------- */


struct DistanceImage{
	int s_height; //height (rows)
	int s_width; //width (columns)
	float hfoV; //horizontal field of view
	float vfoV; //vertical field of view
	Matrix tf_Distance;
	pcl::PointCloud<pcl::PointXYZRGB> CollisionPoints;
};
/* ------------------------------------------------------------------------- */
   /**	\struct DetectedObjects
    *
    *  \brief Auxiliary structure where DetectedObjects coming from the M44_ObjectDetection
    *  function are saved.
    *
    */
   /* ------------------------------------------------------------------------- */
struct DetectedObjects
{
	/* ##### INPUT VARIABLES ##### */
    Matrix m_Objects;
	/*i_NumberOfObjects -> MISSING*/
    int i_NumberOfObjects;
    std::set<int> s_labels;
	int i_status;

	DetectedObjects(int Nrows, int Ncols)
    {
		m_Objects = Matrix(Nrows,Ncols);
		i_NumberOfObjects = 0;
		i_status = 0;
		s_labels.clear();
    };
};
/* ------------------------------------------------------------------------- */
/**	\struct BackgroundReference
 *
 *  \brief Auxiliary structure where DetectedObjects coming from the M44_ObjectDetection
 *  function are saved.
 *

------------------------------------------------------------------------- */

struct BackgroundReference
{
	/* ##### INPUT VARIABLES ##### */
Matrix tf_Dref;
Matrix tf_Vref;
//BackgroundReference();
//BackgroundReference(int Nrows, int Ncols)
//{
//	tf_Dref = Matrix(Nrows, Ncols);
//	tf_Vref = Matrix(Nrows, Ncols);
//}
};

/* ------------------------------------------------------------------------- */
/**	\class cl_DynamicObjects
 *
 *  \brief Class used to store DynamicObjectsLists
 *

------------------------------------------------------------------------- */

class cl_PhysCharac{
public:
	float f_PhysSize;
	float Color;
};
class cl_Pos3D{
public:
	float f_x;
	float f_y;
	float f_z;
};
class cl_Vel3D{
public:
	double d_vx;
	double d_vy;
	double d_vz;
};

class cl_DynamicObjectList
{
public:
	ros::Time r_DetectionTime;
	float f_Identification;
	cl_PhysCharac PhysCharac;
	cl_Pos3D Pos3D;
	cl_Vel3D Vel3D;

};
//struct DynamicObjectList_op{
//	bool operator()(cl_DynamicObjectList l1,cl_DynamicObjectList l2) const
//	{
//		bool res;
//		if (l1.f_Identification > l2.f_Identification)
//			res = true;
//		return res;
//	}
//};
//typedef std::set<cl_DynamicObjectList,DynamicObjectList_op> DynamicObjectListSet;
//
//
//
//class cl_DynamicObjects{
//public:
//	DynamicObjectListSet List;
//	int s_RobotID;
//	cl_DynamicObjects(){
//		List.clear();
//		s_Size = 0;
//	};
//	int getSize(){
//		s_Size = List.size();
//		return s_Size;
//	};
//protected:
//	int s_Size;
//
//
//};

class cl_DynamicObjects2{
public:
	std::vector<cl_DynamicObjectList> List;
	int s_RobotID;
	float maxID;
	cl_DynamicObjects2(){
		List.clear();
		s_Size = 0;
		maxID = 0;
	};
	cl_DynamicObjects2(int size){
		List.reserve(size);
		s_Size = size;
	}
	int getSize(){
		s_Size = List.size();
		return s_Size;
	};
protected:
	int s_Size;

};

/* ------------------------------------------------------------------------- */
/**	\struct auxMapCell
 *
 *  \brief struct used to store maps.
 *

------------------------------------------------------------------------- */
struct auxMapCell
   {
       /* ##### INPUT VARIABLES ##### */

       //public byte[,] Confidence;
		Matrix Confidence;
       //public byte[,] Occupation;
       Matrix Measured_Heights;
       //public byte[,] Communication_Quality;
       //public byte[,] GPS_reception_quality;
       Matrix Measured_Gradient_X;
       Matrix Measured_Gradient_Y;
       //public byte[,] Freedom_probability;
       //public float[,] Distance;
       /*!s_Nrows TBD -> MISSING*/
       int s_Nrows;
       /*!s_Ncols TBD -> MISSING*/
       int s_Ncols;
       /*!us_Scale_local TBD -> MISSING*/
       auxMapCell();
       auxMapCell(int Nrows, int Ncols)
       {
           Confidence = Matrix(Nrows, Ncols);
         //  Occupation = new byte[Nrows, Ncols];
           Measured_Heights = Matrix(Nrows, Ncols);
          // Communication_Quality = new byte[Nrows, Ncols];
          // GPS_reception_quality = new byte[Nrows, Ncols];
           Measured_Gradient_X = Matrix(Nrows, Ncols);
           Measured_Gradient_Y = Matrix(Nrows, Ncols);
          // Freedom_probability = new byte[Nrows, Ncols];
          // Distance = new float[Nrows, Ncols];
           s_Nrows = Nrows;
           s_Ncols = Ncols;
       }
   };

struct cl_MovementData{
    Matrix tb_MovementMap_k;
    Matrix tb_MovementMap_k_1;
    auxMapCell MapCellMatrix_k;
    cl_Pos3D Center_k;
    cl_Pos3D Center_k_1;
    bool b_isMovement;

    cl_MovementData(int Nrows, int Ncols)
     {
         tb_MovementMap_k = Matrix(Nrows, Ncols);
         tb_MovementMap_k_1 = Matrix(Nrows, Ncols);
         b_isMovement = false;
         MapCellMatrix_k = auxMapCell(Nrows, Ncols);
//         Center_k = new cl_Pos3D();
//         Center_k_1 = new cl_Pos3D();
     }
};




}


